#include <TimerThree.h>
#include <TimerOne.h>
#include "math.h"
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <MotorShield.h>
#include <HC_SR04.h>
#include <FlameSensor.h>
#include <Goertzel.h>

// Creates the motor shield object with the default I2C address
MotorShield ms1 = MotorShield(3,4,1,2);

// Creates ultrasonic sensor objects, parameters are pin numbers
HC_SR04 us1(30,31);
HC_SR04 us2(32,33);
HC_SR04 us3(34,35);
HC_SR04 us4(36,37);
HC_SR04 us5(38,39);
HC_SR04 us6(40,41);

// Creates flame sensor object
FlameSensor fs1(A0);

// Creates push button variables
int pbIn = 0;

// Creates microphone variables
int sensorPin = A1;
const float TARGET_FREQUENCY1 = 3800; 
const float TARGET_FREQUENCY2 = 2500; 
const int N = 100;
const float THRESHOLD1 = 4000;
const float THRESHOLD2 = 4000;
const float SAMPLING_FREQUENCY = 8900;
Goertzel goertzel1 = Goertzel(TARGET_FREQUENCY1, N, SAMPLING_FREQUENCY);
Goertzel goertzel2 = Goertzel(TARGET_FREQUENCY2, N, SAMPLING_FREQUENCY);

int motorspeed=128;
boolean debug=false, usesCm=false;
// ultrasonic variables, circular arrays
long cm, d1, d2, d3, d4, d5, d6, diff23, d_temp;
int sens_arr1[10], sens_arr2[10], sens_arr3[10], sens_arr4[10], sens_arr5[10], sens_arr6[10];
int sens_add1=0, sens_add2=0, sens_add3=0, sens_add4=0, sens_add5=0, sens_add6=0, circarr_size=10;

// variables for when robot is turning
boolean turningRight=false, turningLeft=false;

// Jeff's navigation variables
int NINETY_DEGREE_DELAY = 1020;
int UNIT_DISTANCE_DELAY = -1;
int CM_DISTANCE_DELAY = 50; //TODO: Measure this
int maxWallDistance = -1;
long WALL_REACHED_DISTANCE;
long ROBOT_LENGTH = 20/2.54;
int INCHES_DISTANCE_DELAY = 60; //TODO: Measure this
long cornerWallDistance;

boolean cornerWallDetected = false;

int led = 13, count=0;

//------------------------------------------------------------------------------------------
void setup(){
  // attach interrupt to executive function
  Timer3.initialize(); // initializes timer
  
  Serial.begin(9600);
  ms1.begin();
  fs1.setDebugOn();
  
  pinMode(led, OUTPUT);
  // this is the button interrupt
  attachInterrupt(pbIn, buttonInput, RISING);
  
  Timer3.attachInterrupt(goertzel, 125000);
  
  // Jeff's navi initialization
  if(usesCm){
    UNIT_DISTANCE_DELAY = CM_DISTANCE_DELAY;
    maxWallDistance = 30;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 5;
    ROBOT_LENGTH = 20; //TODO: Measure this
  }
  
  else{
    UNIT_DISTANCE_DELAY = INCHES_DISTANCE_DELAY;
    maxWallDistance = 30/2.54;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 4;
    ROBOT_LENGTH = 20/2.54; //TODO: Measure this
  }
}
//---------------------------------------------
void loop(){
  // interrupts will be called
}
//---------------------------------------------
void executive(){
  /* 
    for I2C to work within an interrupt function, interrupts must
    be enabled within the function.  sei() enables interrupts, and
    cli() disables interrupts
  */
  interrupts();
  Serial.print("\nEXEC\n"); // print once executive is reached
  
  // call ultrasonics
  get_distance_all();
  // call flame sensor
  flameDetection();
  // call navigation
  //navigation();
  
  
  Serial.println();
  noInterrupts();
}
//------------------------------------------------------------------------------------------
// this is the button interrupt function
void buttonInput(){
  Serial.println("button pressed");
  // remove button interrupt and mic interrupt so debounce won't crash program
  detachInterrupt(0);
  Timer3.detachInterrupt();
  // enable interrupts so executive can be called
  interrupts();
  // now we can call the executive
  Timer3.attachInterrupt(executive,100000);
}
//------------------------------------------------------------------------------------------
void goertzel(){
  threeEightFreq();
  twoFiveFreq();
}
void threeEightFreq(){
  goertzel1.sample(sensorPin); //Will take n samples
  float magnitude1 = goertzel1.detect();  //check them for target_freq
  if(magnitude1>THRESHOLD1){ //if you're getting false hits or no hits adjust this
    Serial.println("3.8KHz FOUND");
    detachInterrupt(0);
    Timer3.detachInterrupt();
    interrupts();
    Timer3.attachInterrupt(executive,100000);
  }
}
void twoFiveFreq(){
  goertzel2.sample(sensorPin); //Will take n samples
  float magnitude2 = goertzel2.detect();  //check them for target_freq
  if(magnitude2>THRESHOLD2){ //if you're getting false hits or no hits adjust this
    Serial.println("2.5KHz FOUND");
    detachInterrupt(0);
    Timer3.detachInterrupt();
    interrupts();
    Timer3.attachInterrupt(executive,100000);
  }
}
//------------------------------------------------------------------------------------------
void navigation(){
  /*
    if the robot is turning, don't do maze logic
    this accomplishes turning w/o delays
  */
  // robot is turning right, complete turn before going back to maze logic
  if(turningRight){
    // turn right until compass reading shows 90deg turn
    // ms1.turnRight(128)
    
  }
  // robot is turning left, complete turn before going back to maze logic
  else if(turningLeft){
    // turn left until compass reading shows 90deg turn
    // ms1.turnLeft(128)
    
  }
  // not turning, do maze logic
  else{
    // navi main logic algorithm
    
    // check if there is a wall immediately in front of the robot
    if(d1 < WALL_REACHED_DISTANCE){
      /* TURN LEFT */
      turningLeft = true;
    }
    else{
      // now time to move forward
      // compare d2 and d3, determine if angle correction is needed
      diff23 = d2 - d3;
      diff23 = abs(diff23);
      
      // if difference is <2in, move forward with some offset
      if(diff23 < 2 && diff23 >= 0.25){
        // veer to right
        if(d2 > d3){
          /* TODO: ADD FORWARD CORRECTION TO MOTOR LIBRARY */
          //ms1.moveForwardOffset();
        }
        // veer to left
        else if(d3 > d2){
          /* TODO: ADD FORWARD CORRECTION TO MOTOR LIBRARY */
          //ms1.moveForwardOffset();
        }
      }
      
      // else, robot is parallel to wall, just move forward
      else{
        ms1.moveForward(128);
      }
    }
    
    // now check if there is space to the right of the robot
    
    // measure distance from d2 and d3 to wall
    while(d2 > maxWallDistance && d2 > (d3 + maxWallDistance)){
      // now robot is at a corner: d2 can see into a room but d3 has a wall in front of it
      cornerWallDetected = true;
      // move robot forward slowly until past wall
      /* 
        TODO: D3 MUST UPDATE READING EACH TIME
        IS THIS POSSIBLE WITH EXECUTIVE?
      */
      cornerWallDistance = d3;
      ms1.moveForward(32);
    }
    
    // robot moved past opening, inch forward just one more time to clear opening
    if(cornerWallDetected){
      /*
        TODO: MOVE FORWARD JUST A LITTLE BIT
        POSSIBLE WITHOUT DELAYS?
      */
      
    }
    
    // now both d2 and d3 are past the corner
    // turn the robot to the right
    if(d2 > maxWallDistance && d3 > maxWallDistance){
      // turn right then move forward
      
    }
    
    
  }
}
//------------------------------------------------------------------------------------------
void flameDetection(){
  // this will print if flame is found or not
  if(fs1.isFlame()){
    Serial.println("Yes Flame");
  }
  else{
    Serial.println("No Flame");
  }
}
//------------------------------------------------------------------------------------------
// this function will get readings from all ultrasonic sensors
void get_distance_all(){
  // this will get the distance measurement from each sensor
  // TODO: ROUND FRACTION TO NEAREST WHOLE NUMBER; DON'T JUST DROP REMAINDER
  
  d1 = us1.getDistance(usesCm);
  add_sens_arr(1);
  //delay(60);
  
  d2 = us2.getDistance(usesCm);
  add_sens_arr(2);
  //delay(60);
  
  d3 = us3.getDistance(usesCm);
  add_sens_arr(3);
  //delay(60);
  
  /*d4 = us4.getDistance(usesCm);
  add_sens_arr(4);
  //delay(60);
  
  d5 = us5.getDistance(usesCm);
  add_sens_arr(5);
  //delay(60);
  
  d6 = us6.getDistance(usesCm);
  add_sens_arr(6);
  //delay(60);*/
}
//------------------------------------------------------------------------------------------
// this function will get a reading from the specified ultrasonic sensor
void get_distance(int sens_num){
  if(sens_num == 1){
    d1 = us1.getDistance(usesCm);
    add_sens_arr(1);
    //delay(60);
  }
  else if(sens_num == 2){
    d2 = us2.getDistance(usesCm);
    add_sens_arr(2);
    //delay(60);
  }
  else if(sens_num == 3){
    d3 = us3.getDistance(usesCm);
    add_sens_arr(3);
    //delay(60);
  }
  else if(sens_num == 4){
    d4 = us4.getDistance(usesCm);
    add_sens_arr(4);
    //delay(60);
  }
  else if(sens_num == 5){
    d5 = us5.getDistance(usesCm);
    add_sens_arr(5);
    //delay(60);
  }
  else if(sens_num == 6){
    d6 = us6.getDistance(usesCm);
    add_sens_arr(6);
    //delay(60);
  }
}
//------------------------------------------------------------------------------------------
// this function will print the sensor distance, used for debugging
void print_distance(int sens_num){
  if(sens_num == 1){
    Serial.print("d1: ");
    Serial.println(d1);
  }
  else if(sens_num == 2){
    Serial.print("d2: ");
    Serial.println(d2);
  }
  else if(sens_num == 3){
    Serial.print("d3: ");
    Serial.println(d3);
  }
  else if(sens_num == 4){
    Serial.print("d4: ");
    Serial.println(d4);
  }
  else if(sens_num == 5){
    Serial.print("d5: ");
    Serial.println(d5);
  }
  else if(sens_num == 6){
    Serial.print("d6: ");
    Serial.println(d6);
  }
}
//------------------------------------------------------------------------------------------
// this function implements circular storage arrays for the ultrasonic sensor values
// this function adds value to the array when a reading is taken
void add_sens_arr(int sens_num){
  if(sens_num == 1){
    sens_arr1[sens_add1] = d1;
    sens_add1 += 1;
    sens_add1 = (sens_add1 % circarr_size);
  }
  else if(sens_num == 2){
    sens_arr2[sens_add2] = d2;
    sens_add2 += 1;
    sens_add2 = (sens_add2 % circarr_size);
  }
  else if(sens_num == 3){
    sens_arr3[sens_add3] = d3;
    sens_add3 += 1;
    sens_add3 = (sens_add3 % circarr_size);
  }
  else if(sens_num == 4){
    sens_arr4[sens_add4] = d4;
    sens_add4 += 1;
    sens_add4 = (sens_add4 % circarr_size);
  }
  else if(sens_num == 5){
    sens_arr5[sens_add5] = d5;
    sens_add5 += 1;
    sens_add5 = (sens_add5 % circarr_size);
  }
  else if(sens_num == 6){
    sens_arr6[sens_add6] = d6;
    sens_add6 += 1;
    sens_add6 = (sens_add6 % circarr_size);
  }
}
//------------------------------------------------------------------------------------------
